#include <gl/glut.h>
#include "GLUtil.h"
#include <vec.h>
#include "Polyhedron.h"
#include <vector>
#include <malloc.h>
#include <math.h>
#include "ConvertUtil.h"
#include "quaternion.h"
#include "RigidBody.h"
#include "PhyEngine.h"
#include <objLoader.h>


using namespace std;

#pragma comment(lib, "imager")


static GLint    ImageWidth=800;
static GLint    ImageHeight=800;

RigidBodyList* bodies;
PhyEngine* engine;

double timestep = 0.01;

void ArenderMesh()
{


	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	//glBegin(GL_TRIANGLES);

	//bodies->at(0)->applyImpulse(Vec3d(0.2, 0.2, 0), Vec3d(0.1, -0.1, 0));
	//bodies->at(0)->applyImpulse(Vec3d(-0.2, -0.2, 0), Vec3d(-0.1, 0.1, 0));

	engine->update(timestep);

	//glScaled(0.1, 0.1, 0.1);

	for (int i=0; i<engine->bodies->size(); i++)
	{
		RigidBody* body = bodies->at(i);

		body->rebuildMatrices();

		glBegin(GL_TRIANGLES);

		for (int j=0; j<body->poly->faceCount; j++)
		{
			Tri tf = body->poly->getFaceFromFace(j);

			Vert ta = body->poly->vertList[tf.a];
			Vert tb = body->poly->vertList[tf.b];
			Vert tc = body->poly->vertList[tf.c];

			Vec3d tva = body->getPointBodyToWorld(Vec3d(ta.x, ta.y, ta.z));
			Vec3d tvb = body->getPointBodyToWorld(Vec3d(tb.x, tb.y, tb.z));
			Vec3d tvc = body->getPointBodyToWorld(Vec3d(tc.x, tc.y, tc.z));

			glVertex3d(tva[0], tva[1], tva[2]);

			glVertex3d(tvb[0], tvb[1], tvb[2]);

			glVertex3d(tvc[0], tvc[1], tvc[2]);
		}

		glEnd();
	}

	Vec3d pp = bodies->at(0)->getPointBodyToWorld(Vec3d(0, -3, -3));

	Vec3d dir = bodies->at(1)->getVectorBodyToWorld(Vec3d(0, 1, 0));

	Vec3d ppp = pp + dir;

	glBegin(GL_LINES);

	glVertex3d(pp[0], pp[1], pp[2]);

	glVertex3d(ppp[0], ppp[1], ppp[2]);

	glEnd();

	glPointSize(5);

	//Vec3d pp = bodies->at(0)->getPointBodyToWorld(Vec3d(0, -3, -3));

	glColor3d(1, 0, 0);

	glBegin(GL_POINTS);

	glVertex3d(pp[0], pp[1], pp[2]);

	glEnd();

	glColor3d(1, 1, 1);

	//glEnd();


}

void simInitialize()
{
	RigidBody* box2 = new RigidBody("platform5x5.obj", 100000, 1);
	RigidBody* box1 = new RigidBody("bunnyS.obj", 1, 0.1);
	RigidBody* box3 = new RigidBody("bunnyS.obj", 1, 0.1);
	//RigidBody* box3 = new RigidBody("platform.obj", 100000, 1);
	//RigidBody* box4 = new RigidBody();
	//RigidBody* box5 = new RigidBody();
	//RigidBody* box6 = new RigidBody();

	printf("%f\n", box2->mass);

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			//printf("%f ", box2->bodyInertiaMatrix(i, j));
		}
		//printf("\n");
	}

	//box2->mass *= 200;
	//box3->pos = Vec3d(0, -130, -80);
	box2->pos = Vec3d(0, -115, -0);
	//box3->pos = Vec3d(0, -200, -1000);
	//box1->vel = Vec3d(0, -1, 0);
	box1->pos = Vec3d(0, 30, -0);
	box3->pos = Vec3d(0, 40, 0);
	box2->orientation = Quaterniond(0.71, 0, 0, 0.71);
	box1->orientation = Quaterniond(0.71, 0, 0, 0.71);

	//box4->pos = Vec3d(0, 200, -1000);
	//box5->pos = Vec3d(0, 400, -1000);
	//box6->pos = Vec3d(0, 600, -1000);
	//box1->orientation = Quaterniond(0.92, 0, 0, 0.38);
	//box2->orientation = Quaterniond(0.92, 0, 0, 0.38);
	//box1->vel = Vec3d(0, 0.1, 0);
	
	bodies = new RigidBodyList();

	bodies->push_back(box1);
	bodies->push_back(box2);
	bodies->push_back(box3);
	//bodies->push_back(box3);
	//bodies->push_back(box3);
	//bodies->push_back(box4);
	//bodies->push_back(box5);
	//bodies->push_back(box6);
	//bodies->push_back(new RigidBody(box2, Vec3d(0,3,-10), Vec3d(0,0,0), Quaterniond(1,0,0,0), Vec3d(0,0,0), 0.2, 0.5, 1.0));

	//bodies->at(0)->applyImpulse(Vec3d(0.5, 0.5, 0.5), Vec3d(0.1, -0.1, 0));
	//bodies->at(0)->applyImpulse(Vec3d(-0.5, -0.5, -0.5), Vec3d(-0.1, 0.1, 0));

	//printf("%d\n", box1->poly->faceCount * 3 / 2);

	engine = new PhyEngine(bodies, 1, true);
}

void main()
{
	simInitialize();

	//engine->update(timestep);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(ImageWidth, ImageHeight);
	glutCreateWindow("FLIP");
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutKeyboardFunc(keyboard);

	glutMainLoop();
}